/*
 * 础光实时操作系统PhotonRTOS -- 位查找实现文件
 *
 * Copyright (C) 2022, 2023 国科础石(重庆)软件有限公司
 *
 * 作者: Baoyou Xie <xiebaoyou@kernelsoft.com>
 *
 * License terms: GNU General Public License (GPL) version 3
 *
 */

#include <photon/bitops.h>
#include <asm/types.h>

#define BITOP_WORD(nr)		((nr) / BITS_PER_LONG)

uintptr_t find_next_bit(const uintptr_t *addr, uintptr_t size,
			    uintptr_t offset)
{
	const uintptr_t *p = addr + BITOP_WORD(offset);
	uintptr_t result = offset & ~(BITS_PER_LONG-1);
	uintptr_t tmp;

	if (offset >= size) {
		return size;
	}
	size -= result;
	offset %= BITS_PER_LONG;
	if (offset) {
		tmp = *(p++);
		tmp &= (~0UL << offset);
		if (size < BITS_PER_LONG) {
			goto found_first;
		}
		if (tmp) {
			goto found_middle;
		}
		size -= BITS_PER_LONG;
		result += BITS_PER_LONG;
	}
	while (size & ~(BITS_PER_LONG-1)) {
		tmp = *(p++);
		if (tmp) {
			goto found_middle;
		}
		result += BITS_PER_LONG;
		size -= BITS_PER_LONG;
	}
	if (!size) {
		return result;
	}
	tmp = *p;

found_first:
	tmp &= (~0UL >> (BITS_PER_LONG - size));
	if (tmp == 0UL) {		/* Are any bits set? */
		return result + size;	/* Nope. */
	}
found_middle:
	return result + __ffs(tmp);
}

uintptr_t find_next_zero_bit(const uintptr_t *addr, uintptr_t size,
				 uintptr_t offset)
{
	const uintptr_t *p = addr + BITOP_WORD(offset);
	uintptr_t result = offset & ~(BITS_PER_LONG-1);
	uintptr_t tmp;

	if (offset >= size) {
		return size;
	}
	size -= result;
	offset %= BITS_PER_LONG;
	if (offset) {
		tmp = *(p++);
		tmp |= ~0UL >> (BITS_PER_LONG - offset);
		if (size < BITS_PER_LONG) {
			goto found_first;
		}
		if (~tmp) {
			goto found_middle;
		}
		size -= BITS_PER_LONG;
		result += BITS_PER_LONG;
	}
	while (size & ~(BITS_PER_LONG-1)) {
		tmp = *(p++);
		if (~tmp) {
			goto found_middle;
		}
		result += BITS_PER_LONG;
		size -= BITS_PER_LONG;
	}
	if (!size) {
		return result;
	}
	tmp = *p;

found_first:
	tmp |= ~0UL << size;
	if (tmp == ~0UL) {
		return result + size;
	}	/* Nope. */
found_middle:
	return result + ffz(tmp);
}
